#!/usr/bin/env python
#-*- coding:UTF-8 -*-
import rospy
from uav.msg import servo
from gpio_ctrl import gpio_servo_ctrl
import Jetson.GPIO as GPIO
import time

servo_state=[0,0,0]


 
def servo_info_cb(servo_info):

    # 控制对应舵机
    servo_id = int((servo_info.id-33)/2)
    if servo_state[servo_id]==0:
        time.sleep(2)
        gpio_servo_ctrl(servo_info.id, 0.10)   #占空比0.10是投放,0.05是初始位置
        servo_state[servo_id]=1
     
def servo_write():
    
    gpio_servo_ctrl(33, 0.05)   #占空比0.10是投放,0.05是初始位置
    gpio_servo_ctrl(35, 0.05)   #占空比0.10是投放,0.05是初始位置
    gpio_servo_ctrl(37, 0.05)   #占空比0.10是投放,0.05是初始位置
    rospy.init_node('servo_write', anonymous=True)
    rospy.Subscriber("/uav/servo", servo, servo_info_cb)
    rospy.spin()
 
if __name__ == '__main__':
    servo_write()
